#include "pqueue.h"

#define HEAP_SIZE 100

static void heapify(pqueueADT pqueue);
static void heapifyDown(pqueueADT pqueue);
static void expandQueue(pqueueADT pqueue);

/* Types */
struct pqueueCDT {
	int size;
	int numEle;
	int *values;
};

/* Exported entries */
pqueueADT NewPQueue(void) {
	pqueueADT pqueue;
	pqueue = New(pqueueADT);
	pqueue->size = HEAP_SIZE;
	pqueue->numEle = 0;
	pqueue->values = NewArray(HEAP_SIZE, int);
	return pqueue;
}

void FreePQueue(pqueueADT pqueue) {
	FreeBlock(pqueue->values);
	FreeBlock(pqueue);
}

bool IsEmpty(pqueueADT pqueue) {
	return (pqueue->numEle == 0);
}

bool IsFull(pqueueADT pqueue) {
	return FALSE;//(pqueue->size == pqueue->numEle);
}

void Enqueue(pqueueADT pqueue, int newValue) {
	int newIndex;
	
	// 1. Expand queue if needed:
	if (pqueue->size == pqueue->numEle) {
		expandQueue(pqueue);
	}

	// 2. Add the new value to the bottom cell:
	newIndex = pqueue->numEle;
	pqueue->values[newIndex] = newValue;
	pqueue->numEle++;

	// 3. Heapify:
	heapify(pqueue);
}

/* Dequeues the node with the highest priority */
int DequeueMax(pqueueADT pqueue) {
	int i, max = pqueue->values[0], parent, nodeIndex, temp;
	
	// 1. Empty queue:
	if (IsEmpty(pqueue)) {
		Error("Can't dequeue and empty queue...\n");
	}

	// 2. Set root node to the last node:
	pqueue->values[0] = pqueue->values[pqueue->numEle--];

	// 3. Heapify down:
	heapifyDown(pqueue);

	return max;
}

int BytesUsed(pqueueADT pqueue) {
	return (sizeof(*pqueue));
}

/* Heapify the array */
static void heapify(pqueueADT pqueue) {
	int temp, parent, nodeIndex;

	// Bubble up the element to its correct place:
	nodeIndex = pqueue->numEle-1;
	while (nodeIndex > 0) {
		parent = (nodeIndex - 1) / 2;
		// No shifting needed:
		if (pqueue->values[parent] >= pqueue->values[nodeIndex]) {
			break;
		} else {
			temp = pqueue->values[parent];
			pqueue->values[parent] = pqueue->values[nodeIndex];
			pqueue->values[nodeIndex] = temp;
		}
		nodeIndex = parent;
	}
}

/* Heapify the array by bubble down the smallest element (used by DequeueMax()) */
static void heapifyDown(pqueueADT pqueue) {
	int temp, parent, nodeIndex, childLeft, childRight;

	// Bubble DOWN the element to its correct place:
	nodeIndex = 0;
	while (nodeIndex < pqueue->numEle) { //TODO: verify... pqueue->numEle-1
		parent = nodeIndex;
		childLeft = 2 * nodeIndex + 1;
		childRight = childLeft + 1;
		
		if (pqueue->numEle >= 1 && childLeft >= pqueue->numEle )
			break;
		
		// Parent is smaller than one or both children:
		if (pqueue->values[parent] <= pqueue->values[childLeft]
			|| pqueue->values[parent] <= pqueue->values[childRight] )
		{
			// Child left >= child right -> use left child, otherwise -> use right child:
			nodeIndex = (pqueue->values[childLeft] >= pqueue->values[childRight] ? childLeft : childRight );
		
			// Switch with one of its children:
			temp = pqueue->values[parent];
			pqueue->values[parent] = pqueue->values[nodeIndex];
			pqueue->values[nodeIndex] = temp;
		} else {
			break;
		}
	}
}

/* Expands the queue array */
static void expandQueue(pqueueADT pqueue) {
	int i, *oldValues;

	pqueue->size *= 2;
	oldValues = NewArray(pqueue->size, int);
	for (i=0; i<pqueue->size/2; i++) {
		oldValues[i] = pqueue->values[i];
	}
	free(pqueue->values);
	pqueue->values = oldValues;
}